#include "yeyagangcontrol.h"
#include "ui_yeyagangcontrol.h"
#include "DataParser.h"
#include "FloraLogger.h"
#include "qmath.h"
bool YeYaGangControl::isRun = false;
YeYaGangControl::YeYaGangControl(QWidget *parent) :
    QWidget(parent),
    ui(new Ui::YeYaGangControl)
{
    ui->setupUi(this);
}

YeYaGangControl::YeYaGangControl(uint8_t id, int32_t max, int32_t min, int32_t zero, QWidget *parent):m_id(id),max_pos(max),min_pos(min)
  ,zeroOffect(zero),QWidget(parent),
    ui(new Ui::YeYaGangControl)
{
    ui->setupUi(this);
    connect(&controlTakeTimer,&QTimer::timeout,this,&YeYaGangControl::controlTask);


}

YeYaGangControl::~YeYaGangControl()
{
    delete ui;
}

void YeYaGangControl::setName(QString name)
{
    m_name  = name;
    ui->lbName->setText(name);
}

void YeYaGangControl::updateSendPos(int32_t pos)
{
    ui->lbsendPos->setNum(pos);
}

void YeYaGangControl::updateMes(uint32_t mesio)
{
    ui->lbMESID->setText(QString("D:%1").arg(mesio));
}

void YeYaGangControl::enableYeYa(uint16_t enable)
{

}

void YeYaGangControl::setTagertPos(int32_t pos)
{

    if(max_pos !=0 || min_pos !=0)
    {
        if(pos > max_pos)
        {
            pos = max_pos;
        }
        if(pos < min_pos)
        {
            pos = min_pos;
        }
    }
    m_targetPos = pos;
    temp_controlPos = m_curPos;
    LOG_DEBUG<<"zuizhong "<<m_targetPos;
    updateSetPos(m_targetPos);
    //if(isRun)return;
    controlTakeTimer.start(100);
    control_step = FIRST_GETPOS;
    posChange =0;
    isRun = true;

}

void YeYaGangControl::readCurPos()
{
//    for(int i=0; i<20;i++)
//    {
//        auto temp = canopen_sdo_read(i,0xba60,0);
//        emit sendMessage(temp);
//    }

    auto temp = canopen_sdo_read(m_id,REAL_POS_ADDR,0);
    emit sendMessage(temp);
    f_ask_Pos_h = 1;
    f_ask_Pos_l  =1;
}

void YeYaGangControl::readSpeed()
{
    auto temp = canopen_sdo_read(m_id,SPEED_ADDR,0);
    emit sendMessage(temp);
}

void YeYaGangControl::readKaiKou()
{

}

void YeYaGangControl::reciveMessage(can_frame fam)
{
    U32Union u32Parse;
    uint8_t funCode = fam.data[0];
    uint16_t addr = fam.data[1] | (fam.data[2]<<8);
    int32_t data_32;
    switch(addr)
    {
        case REALPOS_H_ADDR:
            temp_CurPos_H = fam.data[4] | (fam.data[5]<<8);
            f_ask_Pos_h =  0;
            if(f_ask_Pos_h ==0 && f_ask_Pos_l == 0)
            {
                u32Parse.u16[0] = temp_CurPos_L;
                u32Parse.u16[1] = temp_CurPos_H;
                m_curPos = u32Parse.i32;
                updateCurPos(m_curPos);
            }
        break;
        case REALPOS_L_ADDR:
            f_ask_Pos_l = 0;
            temp_CurPos_L = fam.data[4] | (fam.data[5]<<8);
        break;
        case REAL_POS_ADDR:
            m_curPos = parse_int16(fam.data[5],fam.data[4]);
            updateCurPos(m_curPos);
            posChange++;
        break;
        case SPEED_ADDR:
            read_speed = parse_int16(fam.data[5],fam.data[4]);

            break;
    }
    mesId+=1;
    updateMes(mesId);
}

void YeYaGangControl::updateCurPos(int32_t pos)
{
    ui->lbCurPos->setNum(pos);
}

void YeYaGangControl::updateSetPos(int32_t pos)
{
    ui->leTagetPos->setText(QString::number(pos));
}

void YeYaGangControl::setZeroPos(int32_t pos)
{

}

void YeYaGangControl::setSpeed_1(int16_t speed)
{
    m_speed = speed;
    if(m_speed <1)m_speed= 1;

}

void YeYaGangControl::controlTask()
{
    switch (control_step) {
    case FIRST_GETPOS:
        readCurPos();
        if(posChange)
        {
            posChange = 0;
            control_step = SEND_POS;
            temp_controlPos = m_curPos;
        }
        break;
    case SEND_POS:
        if(temp_controlPos < m_targetPos)
        {
            temp_controlPos = temp_controlPos + m_speed;
            if(temp_controlPos >= m_targetPos)
            {
                controlTakeTimer.stop();
                temp_controlPos = m_targetPos;
            }
        }
        else if(temp_controlPos > m_targetPos)
        {
            temp_controlPos = temp_controlPos - m_speed;
            if(temp_controlPos <= m_targetPos)
            {
                controlTakeTimer.stop();
                temp_controlPos = m_targetPos;
            }
        }
        if(temp_controlPos == m_targetPos)
        {
            controlTakeTimer.stop();
            isRun = false;
        }
        LOG_DEBUG<<"taget:"<<m_targetPos<<"temp_controlPos :"<<temp_controlPos;
        auto temp = canopen_sdo_write(m_id,SET_POS_ADDR,0,temp_controlPos,WRITE_BYTE_4);
        emit sendMessage(temp);
        break;

    }
    updateSendPos(temp_controlPos);
    readCurPos();
}

void YeYaGangControl::on_btnEnable_clicked()
{
    if(zeroOffect != NOZERO)
    {
        LOG_DEBUG<<"gui ling";
        setTagertPos(zeroOffect);
    }
}

void YeYaGangControl::on_btnAskPos_clicked()
{
    readCurPos();
}

void YeYaGangControl::on_btnSetPos_clicked()
{
    setTagertPos(ui->leTagetPos->text().toInt());
}

void YeYaGangControl::on_btnSetSpeed_clicked()
{

    setSpeed_1(ui->leSpeed->text().toShort());
}

void YeYaGangControl::on_btnAskPos_3_clicked()
{
    readSpeed();
}

void YeYaGangControl::on_btn_stop_clicked()
{

    controlTakeTimer.stop();
    auto temp = canopen_sdo_write(m_id,SET_POS_ADDR,0,m_curPos,WRITE_BYTE_4);
    emit sendMessage(temp);
}
